#include"ccd.h"
#include<cmath>
using std::abs;
void CCD::setup()
{
  FermiSurface=pSystem->FermiSurface;
  totalOrbitals=pSystem->Orbitals.size();
  generateTwoBodyStates();
  generateBlockMatrices();
}
void CCD::generateTwoBodyStates()
{
  TwoBodyStates_hh.clear();
  TwoBodyStates_pp.clear();

  for(int a=0;a<=FermiSurface;a++)
    for(int b=a;b<=FermiSurface;b++)
      {
	const HFOrbital& A=HFOrbs[a];
	const HFOrbital & B=HFOrbs[b];
	int tz=(A.tz + B.tz)/2;
	int par=(A.l + B.l)%2;
	int jmin=abs(A.jj - B.jj)/2;
	int jmax=(A.jj + B.jj)/2;
	for(int j=jmin;j<=jmax;j++)
	  {
      	    if(a==b && (j%2) ) continue;
	    TwoBodyStates_hh.push_back(TwoBodyStateType(a,b,tz,par,j));
	  }
      }
  for(int a=FermiSurface+1;a<totalOrbitals;a++)
    for(int b=a;b<totalOrbitals;b++)
      {
	const HFOrbital & A=HFOrbs[a];
	const HFOrbital & B=HFOrbs[b];
	int tz=(A.tz + B.tz)/2;
	int par=(A.l + B.l)%2;
	int jmin=abs(A.jj - B.jj)/2;
	int jmax=(A.jj + B.jj)/2;
	for(int j=jmin;j<=jmax;j++)
	  {
	    if(a==b && (j%2) ) continue;
	    TwoBodyStates_pp.push_back(TwoBodyStateType(a,b,tz,par,j));
	  }
      }

  sort(TwoBodyStates_hh.begin(),TwoBodyStates_hh.end());
  sort(TwoBodyStates_pp.begin(),TwoBodyStates_pp.end());

  TwoBodyStates_phm.clear();
  TwoBodyStates_hpm.clear();
  for(int a = FermiSurface+1; a <totalOrbitals; a++)
    for(int b = 0; b <= FermiSurface; b++)
      {
	const HFOrbital & A=HFOrbs[a];
	const HFOrbital & B=HFOrbs[b];
	int tz=(A.tz - B.tz)/2;
	int par=abs(A.l - B.l)%2;
	int jmin=abs(A.jj - B.jj)/2;
	int jmax=(A.jj + B.jj)/2;
	for(int j=jmin;j<=jmax;j++)
	  TwoBodyStates_phm.push_back(TwoBodyStateType(a,b,tz,par,j));
      }

  for(int a = 0; a <=FermiSurface; a++)
    for(int b = FermiSurface+1; b <totalOrbitals; b++)
      {
	const HFOrbital & A=HFOrbs[a];
	const HFOrbital & B=HFOrbs[b];
	int tz=(A.tz - B.tz)/2;
	int par=abs(A.l - B.l)%2;
	int jmin=abs(A.jj - B.jj)/2;
	int jmax=(A.jj + B.jj)/2;
	for(int j=jmin;j<=jmax;j++)
	  TwoBodyStates_hpm.push_back(TwoBodyStateType(a,b,tz,par,j));
      }
  sort(TwoBodyStates_phm.begin(),TwoBodyStates_phm.end());
  sort(TwoBodyStates_hpm.begin(),TwoBodyStates_hpm.end());
}
double CCD::cross_coupled1(int a,int b,int c,int d,int J) const
{
  double temp=0;
  int jja=HFOrbs[a].jj;
  int jjb=HFOrbs[b].jj;
  int jjc=HFOrbs[c].jj;
  int jjd=HFOrbs[d].jj;
  int j_min=max(abs(jja-jjb),abs(jjc-jjd))/2;
  int j_max=min(jja+jjb,jjc+jjd)/2;
  if( (HFOrbs[a].tz+HFOrbs[b].tz)!=(HFOrbs[c].tz+HFOrbs[d].tz) ) return 0;
  if( (HFOrbs[a].l+HFOrbs[b].l + HFOrbs[c].l+HFOrbs[d].l)%2 ) return 0;
  if(J<abs(jja-jjc)/2) return 0;
  if(J<abs(jjb-jjd)/2) return 0;
  if(J>(jja+jjc)/2) return 0;
  if(J>(jjb+jjd)/2) return 0;
  
  for(int j=j_min;j<=j_max;j++)
    {
      temp+=(j*2+1)*phase((jjb+jjc)/2+j+J)*gsl_sf_coupling_6j(jjc,jja,2*J,jjb,jjd,2*j)*pSystem->get2B(a,b,c,d,j);      
    }
  return temp;
}
void CCD::generateBlockMatrices()//generate block matrices for ladder and full ccd calculation
{
  CCD_V_hhhh.clear();
  CCD_V_hhpp.clear();
  CCD_V_pppp.clear();
  CCD_V_hpmhpm.clear();
  CCD_V_phmhpm.clear();
  CCD_T_hhpp.clear();
  CCD_e_hhpp.clear();
  CCD_position=Matrix<position,Dynamic,Dynamic>(totalOrbitals,totalOrbitals);
  for(int a=0;a<totalOrbitals;a++)
    for(int b=a;b<totalOrbitals;b++)
      {
	int jja=HFOrbs[a].jj;
	int jjb=HFOrbs[b].jj;
	int count= (jja+jjb)/2-abs(jja-jjb)/2 + 1;
	CCD_position(a,b).channel.resize(count);
	CCD_position(a,b).indice.resize(count);
	for(int j=0;j<count;j++)
	  {
	    CCD_position(a,b).channel[j]=-1;
	  }
      }
  
  //hhpp
  for (int i = 0, j = 0; i < TwoBodyStates_hh.size() && j < TwoBodyStates_pp.size(); i++)
    {
      TwoBodyStateType * State_hh = &TwoBodyStates_hh[i];
      TwoBodyStateType * State_pp = &TwoBodyStates_pp[j];
      while(!State_pp->isInAChannelWith(*State_hh) )
	{
	  j++;
	  if(j>=TwoBodyStates_pp.size() )
	    break;
	  State_pp = &TwoBodyStates_pp[j];
	}
      if( j >= TwoBodyStates_pp.size() )
	break;
      CCD_V_hhpp.push_back( ccd_channel(State_hh->Tz,State_hh->Par,State_hh->J) );
      int channelNumber = CCD_V_hhpp.size()-1;

      CCD_V_hhpp[channelNumber].bra.push_back(i);
      int J=State_hh->J;
      int ha=State_hh->a;
      int hb=State_hh->b;
      int jjha=HFOrbs[ha].jj;
      int jjhb=HFOrbs[hb].jj;
      int n_ab=J-abs(jjha-jjhb)/2;
      CCD_position(ha,hb).channel[n_ab] = channelNumber;
      CCD_position(ha,hb).indice[n_ab] = CCD_V_hhpp[channelNumber].bra.size()-1;

      CCD_V_hhpp[channelNumber].ket.push_back(j);
      int pa=State_pp->a;
      int pb=State_pp->b;
      int jjpa=HFOrbs[pa].jj;
      int jjpb=HFOrbs[pb].jj;
      n_ab=J-abs(jjpa-jjpb)/2;
      CCD_position(pa,pb).channel[n_ab] = channelNumber;
      CCD_position(pa,pb).indice[n_ab] = CCD_V_hhpp[channelNumber].ket.size()-1;

      if( i < TwoBodyStates_hh.size()-1 )
	{
	  TwoBodyStateType * Ph = & TwoBodyStates_hh[i+1];
	  while( (*Ph).isInAChannelWith(*State_hh) )
	    {
	      i++;
	      CCD_V_hhpp[channelNumber].bra.push_back(i);
	      ha=Ph->a;
	      hb=Ph->b;
	      jjha=HFOrbs[ha].jj;
	      jjhb=HFOrbs[hb].jj;
	      n_ab=J-abs(jjha-jjhb)/2;
	      CCD_position(ha,hb).channel[n_ab] = channelNumber;
	      CCD_position(ha,hb).indice[n_ab] = CCD_V_hhpp[channelNumber].bra.size()-1;
	      if( i >= TwoBodyStates_hh.size()-1) break;
	      State_hh = &TwoBodyStates_hh[i];
	      Ph = &TwoBodyStates_hh[i+1];
	    }
	}
      if(j < TwoBodyStates_pp.size()-1 )
	{
	  TwoBodyStateType * Pp = &TwoBodyStates_pp[j+1];
	  while( (*Pp).isInAChannelWith(*State_pp) )
	    {
	      j++;
	      CCD_V_hhpp[channelNumber].ket.push_back(j);
	      pa=Pp->a;
	      pb=Pp->b;
	      jjpa=HFOrbs[pa].jj;
	      jjpb=HFOrbs[pb].jj;
	      n_ab=J-abs(jjpa-jjpb)/2;
	      CCD_position(pa,pb).channel[n_ab] = channelNumber;
	      CCD_position(pa,pb).indice[n_ab] = CCD_V_hhpp[channelNumber].ket.size()-1;

	      if(j >= TwoBodyStates_pp.size()-1) break;
	      State_pp = &TwoBodyStates_pp[j];
	      Pp = &TwoBodyStates_pp[j+1];
	    }
	}
      j++;
    }

  //hhhh
  for(int i = 0; i < CCD_V_hhpp.size(); i++)
    {
      int tz = CCD_V_hhpp[i].Tz;
      int par = CCD_V_hhpp[i].Par;
      int j = CCD_V_hhpp[i].J;
      CCD_V_hhhh.push_back(ccd_channel(tz,par,j));
      int size = CCD_V_hhpp[i].bra.size();
      CCD_V_hhhh[CCD_V_hhhh.size()-1].bra.resize(size);
      CCD_V_hhhh[CCD_V_hhhh.size()-1].ket.resize(size);
      for(int j = 0; j < size; j++)
  	{
  	  CCD_V_hhhh[CCD_V_hhhh.size()-1].bra[j] = CCD_V_hhpp[i].bra[j];
  	  CCD_V_hhhh[CCD_V_hhhh.size()-1].ket[j] = CCD_V_hhpp[i].bra[j];
  	}
    }

  //pppp
  for(int i = 0; i < CCD_V_hhpp.size(); i++)
    {
      int tz = CCD_V_hhpp[i].Tz;
      int par = CCD_V_hhpp[i].Par;
      int j = CCD_V_hhpp[i].J;
      CCD_V_pppp.push_back(ccd_channel(tz,par,j));
      int size = CCD_V_hhpp[i].ket.size();
      CCD_V_pppp[CCD_V_pppp.size()-1].bra.resize(size);
      CCD_V_pppp[CCD_V_pppp.size()-1].ket.resize(size);
      for(int j = 0; j < size; j++)
  	{
  	  CCD_V_pppp[CCD_V_pppp.size()-1].bra[j] = CCD_V_hhpp[i].ket[j];
  	  CCD_V_pppp[CCD_V_pppp.size()-1].ket[j] = CCD_V_hhpp[i].ket[j];
  	}
    }
  
  //interaction for hhpp
  CCD_T_hhpp = CCD_V_hhpp;
  CCD_e_hhpp = CCD_V_hhpp;
  double temp=0;
  for(int i = 0; i < CCD_V_hhpp.size(); i++)
    {
      int j=CCD_V_hhpp[i].J;
      int bra_dim = CCD_V_hhpp[i].bra.size();
      int ket_dim = CCD_V_hhpp[i].ket.size();
      CCD_V_hhpp[i].Mat.resize(bra_dim,ket_dim);
      CCD_T_hhpp[i].Mat.resize(bra_dim,ket_dim);
      CCD_e_hhpp[i].Mat.resize(bra_dim,ket_dim);
      for(int Ibra=0; Ibra < bra_dim; Ibra++)
  	{
  	  for(int Iket = 0; Iket < ket_dim; Iket++)
  	    {
  	      TwoBodyStateType & State_bra = TwoBodyStates_hh[ CCD_V_hhpp[i].bra[Ibra] ];
  	      TwoBodyStateType & State_ket = TwoBodyStates_pp[ CCD_V_hhpp[i].ket[Iket] ];
  	      int bra_a = State_bra.a;
  	      int bra_b = State_bra.b;
  	      int ket_a = State_ket.a;
  	      int ket_b = State_ket.b;
	      double factor=1.0;
	      if(bra_a==bra_b)
		factor*=sqrt(0.5);
	      if(ket_a==ket_b)
		factor*=sqrt(0.5);
  	      double V = pSystem->get2B(bra_a,bra_b,ket_a,ket_b,j)*factor;
	      double de = 1.0/(HFOrbs[bra_a].e + HFOrbs[bra_b].e - HFOrbs[ket_a].e - HFOrbs[ket_b].e);
	      CCD_V_hhpp[i].Mat(Ibra,Iket) = V;
	      CCD_e_hhpp[i].Mat(Ibra,Iket) = de;
	      CCD_T_hhpp[i].Mat(Ibra,Iket) = V*de;
	    }
	}
    }

  //interaction for hhhh
  for(int i = 0; i < CCD_V_hhhh.size(); i++)
    {
      int j=CCD_V_hhhh[i].J;
      int dim = CCD_V_hhhh[i].bra.size();
      CCD_V_hhhh[i].Mat.resize(dim,dim);
      for(int Ibra=0; Ibra < dim; Ibra++)
	{
	  for(int Iket = Ibra; Iket < dim; Iket++)
	    {
	      TwoBodyStateType & State_bra = TwoBodyStates_hh[ CCD_V_hhhh[i].bra[Ibra] ];
	      TwoBodyStateType & State_ket = TwoBodyStates_hh[ CCD_V_hhhh[i].ket[Iket] ] ;
	      int bra_a = State_bra.a;
	      int bra_b = State_bra.b;
	      int ket_a = State_ket.a;
	      int ket_b = State_ket.b;
	      double factor=1.0;
	      if(bra_a==bra_b)
		factor*=sqrt(0.5);
	      if(ket_a==ket_b)
		factor*=sqrt(0.5);
	      CCD_V_hhhh[i].Mat(Ibra,Iket) = pSystem->get2B(bra_a,bra_b,ket_a,ket_b,j)*factor;
	      CCD_V_hhhh[i].Mat(Iket,Ibra) = CCD_V_hhhh[i].Mat(Ibra,Iket);
	    }
	}
    }

  //interaction for pppp
  for(int i = 0; i < CCD_V_pppp.size(); i++)
    {
      int j=CCD_V_pppp[i].J;
      int dim = CCD_V_pppp[i].bra.size();
      CCD_V_pppp[i].Mat.resize(dim,dim);
      for(int Ibra = 0; Ibra < dim; Ibra++)
	{
	  for(int Iket = Ibra; Iket < dim; Iket++)
	    {
	      TwoBodyStateType & State_bra = TwoBodyStates_pp[ CCD_V_pppp[i].bra[Ibra] ];
	      TwoBodyStateType & State_ket = TwoBodyStates_pp[ CCD_V_pppp[i].ket[Iket] ];
	      int bra_a = State_bra.a;
	      int bra_b = State_bra.b;
	      int ket_a = State_ket.a;
	      int ket_b = State_ket.b;
	      double factor=1.0;
	      if(bra_a==bra_b)
		factor*=sqrt(0.5);
	      if(ket_a==ket_b)
		factor*=sqrt(0.5);
	      CCD_V_pppp[i].Mat(Ibra,Iket) = pSystem->get2B(bra_a,bra_b,ket_a,ket_b,j)*factor;
	      CCD_V_pppp[i].Mat(Iket,Ibra) = CCD_V_pppp[i].Mat(Ibra,Iket);
	    }
	}
    }
  
  //phmhpm
  for (int i = 0, j = 0; i < TwoBodyStates_phm.size() && j < TwoBodyStates_hpm.size(); i++)
    {
      TwoBodyStateType * State_phm = &TwoBodyStates_phm[i];
      TwoBodyStateType * State_hpm = &TwoBodyStates_hpm[j];
      while( !State_hpm->isInAChannelWith(*State_phm) )
  	{
  	  j++;
  	  if(j >= TwoBodyStates_hpm.size() )
  	    break;
  	  State_hpm = &TwoBodyStates_hpm[j];
  	}
      if( j >= TwoBodyStates_hpm.size() )
  	break;

      CCD_V_phmhpm.push_back( ccd_channel(State_phm->Tz,State_phm->Par,State_phm->J) );
      int channelNumber = CCD_V_phmhpm.size()-1;

      CCD_V_phmhpm[channelNumber].bra.push_back(i);

      CCD_V_phmhpm[channelNumber].ket.push_back(j);

      if( i < TwoBodyStates_phm.size()-1 )
	{
	  TwoBodyStateType * Ph = &TwoBodyStates_phm[i+1];
	  while( (*Ph).isInAChannelWith(*State_phm) )
	    {
	      i++;
	      CCD_V_phmhpm[channelNumber].bra.push_back(i);
	      if( i >= TwoBodyStates_phm.size()-1) break;
	      State_phm = &TwoBodyStates_phm[i];
	      Ph = &TwoBodyStates_phm[i+1];
	    }
	}
      if(j < TwoBodyStates_hpm.size()-1 )
	{
	  TwoBodyStateType * Pp = &TwoBodyStates_hpm[j+1];
	  while( (*Pp).isInAChannelWith(*State_hpm) )
	    {
	      j++;
	      CCD_V_phmhpm[channelNumber].ket.push_back(j);
	      if(j >= TwoBodyStates_hpm.size()-1) break;
	      State_hpm = &TwoBodyStates_hpm[j];
	      Pp = &TwoBodyStates_hpm[j+1];
	    }
	}
      j++;
    }

  //hpmhpm
  for(int i = 0; i < CCD_V_phmhpm.size(); i++)
    {
      int tz = CCD_V_phmhpm[i].Tz;
      int par = CCD_V_phmhpm[i].Par;
      int j = CCD_V_phmhpm[i].J;
      CCD_V_hpmhpm.push_back(ccd_channel(tz,par,j));
      int size = CCD_V_phmhpm[i].ket.size();
      CCD_V_hpmhpm[CCD_V_hpmhpm.size()-1].bra.resize(size);
      CCD_V_hpmhpm[CCD_V_hpmhpm.size()-1].ket.resize(size);
      for(int j = 0; j < size; j++)
  	{
  	  CCD_V_hpmhpm[CCD_V_hpmhpm.size()-1].bra[j] = CCD_V_phmhpm[i].ket[j];
  	  CCD_V_hpmhpm[CCD_V_hpmhpm.size()-1].ket[j] = CCD_V_phmhpm[i].ket[j];
  	}
    }


  //interaction for phmhpm
  /*
    <p1 h1^{-1},J|h2 p2^{-1},J > define as (-1)^{jp2-jh2+J} cross_coupled1(p1,p2 , h1 h2,J)
  */

  for(int i = 0; i < CCD_V_phmhpm.size(); i++)
    {
      int J=CCD_V_phmhpm[i].J;
      int bra_dim = CCD_V_phmhpm[i].bra.size();
      int ket_dim = CCD_V_phmhpm[i].ket.size();
      CCD_V_phmhpm[i].Mat.resize(bra_dim,ket_dim);
      for(int Ibra=0; Ibra < bra_dim; Ibra++)
  	{
  	  for(int Iket = 0; Iket < ket_dim; Iket++)
  	    {
  	      TwoBodyStateType & State_bra = TwoBodyStates_phm[ CCD_V_phmhpm[i].bra[Ibra] ];
  	      TwoBodyStateType & State_ket = TwoBodyStates_hpm[ CCD_V_phmhpm[i].ket[Iket] ];
  	      int p1 = State_bra.a;//p1
  	      int h1 = State_bra.b;//h1
  	      int h2 = State_ket.a;//h2
  	      int p2 = State_ket.b;//p2
  	      int jjh2=HFOrbs[h2].jj;
  	      int jjp2=HFOrbs[p2].jj;	      
  	      int phase=((jjh2-jjp2)/2+J)%2?-1:1;	      
  	      CCD_V_phmhpm[i].Mat(Ibra,Iket) = phase*cross_coupled1(p1,p2,h1,h2,J);
  	    }
  	}
    }
  
  //interaction for hpmhpm
  /*   
       < h1 p2^{-1}|h2 p1^{-1},J> define as (-1)^{jh2-jp1+J} cross_coupled1(h1,p1,p2,h2,J)

  */
  for(int i = 0; i < CCD_V_hpmhpm.size(); i++)
    {
      int J=CCD_V_hpmhpm[i].J;
      int dim = CCD_V_hpmhpm[i].bra.size();
      CCD_V_hpmhpm[i].Mat.resize(dim,dim);
      for(int Ibra = 0; Ibra < dim; Ibra++)
  	{
  	  for(int Iket = Ibra; Iket < dim; Iket++)
  	    {
  	      TwoBodyStateType & State_bra = TwoBodyStates_hpm[ CCD_V_hpmhpm[i].bra[Ibra] ];
  	      TwoBodyStateType & State_ket = TwoBodyStates_hpm[ CCD_V_hpmhpm[i].ket[Iket] ];
  	      int h1 = State_bra.a;//h1
  	      int p2 = State_bra.b;//p2
  	      int h2 = State_ket.a;//h2
  	      int p1 = State_ket.b;//p1
  	      int jjh2=HFOrbs[h2].jj;
  	      int jjp1=HFOrbs[p1].jj;
  	      int phase=((jjh2-jjp1)/2+J)%2?-1:1;
  	      CCD_V_hpmhpm[i].Mat(Ibra,Iket) = phase*cross_coupled1(h1,p1,p2,h2,J);
  	      CCD_V_hpmhpm[i].Mat(Iket,Ibra) = CCD_V_hpmhpm[i].Mat(Ibra,Iket);
  	    }
  	}
    }  
}

double CCD::gethhpp(int i,int j,int a,int b,int J,const vector<ccd_channel> & hhpp)
{
  int jjh1=HFOrbs[i].jj;
  int jjh2=HFOrbs[j].jj;
  int jjp1=HFOrbs[a].jj;
  int jjp2=HFOrbs[b].jj;
  int jmin_h=abs(jjh1-jjh2)/2;
  int jmax_h=(jjh1+jjh2)/2;
  int jmin_p=abs(jjp1-jjp2)/2;
  int jmax_p=(jjp1+jjp2)/2;
  int jmin=max(jmin_h,jmin_p);
  int jmax=min(jmax_h,jmax_p);
  if(jmin>jmax) return 0;
  if( i==j && (J%2) ) return 0;
  if( a==b && (J%2) ) return 0;

  double factor=1.0;
  if(a==b) factor*=sqrt(2.);
  if(i==j) factor*=sqrt(2.);
  
  int index_h=J-jmin_h;
  int index_p=J-jmin_p;
  double phase=1.0;
  position * pos_pp;
  position * pos_hh;
  if(a>b)
    {
      pos_pp = &CCD_position(b,a);
      phase*=( (jjp1+jjp2)/2+J+1 )%2?-1:1;
    }
  else
    {
      pos_pp= &CCD_position(a,b);
    }
  if(i > j)
    {
      pos_hh = &CCD_position(j,i);
      phase*=( (jjh1+jjh2)/2+J+1 )%2?-1:1;
    }
  else
    {
      pos_hh = &CCD_position(i,j);
    }
  if (pos_pp->channel[index_p] != -1&&pos_hh->channel[index_h] != -1&&pos_pp->channel[index_p]==pos_hh->channel[index_h])
    return phase*factor*hhpp[pos_pp->channel[index_p]].Mat(pos_hh->indice[index_h],pos_pp->indice[index_p]);
  else return 0;
}
double CCD::getpppp(int a,int b,int c,int d,int J,const vector<ccd_channel> & pppp)
{
  int jjp1=HFOrbs[a].jj;
  int jjp2=HFOrbs[b].jj;
  int jjp3=HFOrbs[c].jj;
  int jjp4=HFOrbs[d].jj;
  int jmin_bra=abs(jjp1-jjp2)/2;
  int jmax_bra=(jjp1+jjp2)/2;
  int jmin_ket=abs(jjp3-jjp4)/2;
  int jmax_ket=(jjp3+jjp4)/2;
  int jmin=max(jmin_bra,jmin_ket);
  int jmax=min(jmax_bra,jmax_ket);
  if(jmin>jmax) return 0;

  if( c==d && (J%2) ) return 0;
  if( a==b && (J%2) ) return 0;

  double factor=1.0;
  if(a==b) factor*=sqrt(2.);
  if(c==d) factor*=sqrt(2.);
  
  int index_bra=J-jmin_bra;
  int index_ket=J-jmin_ket;
  double phase=1.0;
  position * pos_bra;
  position * pos_ket;
  if(a>b)
    {
      pos_bra = &CCD_position(b,a);
      phase*=( (jjp1+jjp2)/2+J+1 )%2?-1:1;
    }
  else
    {
      pos_bra= &CCD_position(a,b);
    }
  if(c > d)
    {
      pos_ket = &CCD_position(d,c);
      phase*=( (jjp3+jjp4)/2+J+1 )%2?-1:1;
    }
  else
    {
      pos_ket = &CCD_position(c,d);
    }
  if (pos_bra->channel[index_bra] != -1&&pos_ket->channel[index_ket] != -1&&pos_bra->channel[index_bra]==pos_ket->channel[index_ket])
    return phase*factor*pppp[pos_bra->channel[index_bra]].Mat(pos_bra->indice[index_bra],pos_ket->indice[index_ket]);
  else return 0;
}
double CCD::gethhhh(int i,int j,int k,int l,int J,const vector<ccd_channel> & hhhh)
{
  int jjh1=HFOrbs[i].jj;
  int jjh2=HFOrbs[j].jj;
  int jjh3=HFOrbs[k].jj;
  int jjh4=HFOrbs[l].jj;
  int jmin_bra=abs(jjh1-jjh2)/2;
  int jmax_bra=(jjh1+jjh2)/2;
  int jmin_ket=abs(jjh3-jjh4)/2;
  int jmax_ket=(jjh3+jjh4)/2;
  int jmin=max(jmin_bra,jmin_ket);
  int jmax=min(jmax_bra,jmax_ket);
  if(jmin>jmax) return 0;

  if( i==j && (J%2) ) return 0;
  if( k==l && (J%2) ) return 0;

  double factor=1.0;
  if(i==j) factor*=sqrt(2.);
  if(k==l) factor*=sqrt(2.);
  
  int index_bra=J-jmin_bra;
  int index_ket=J-jmin_ket;
  double phase=1.0;
  position * pos_bra;
  position * pos_ket;
  if(i>j)
    {
      pos_bra = &CCD_position(j,i);
      phase*=( (jjh1+jjh2)/2+J+1 )%2?-1:1;
    }
  else
    {
      pos_bra= &CCD_position(i,j);
    }
  if(k > l)
    {
      pos_ket = &CCD_position(l,k);
      phase*=( (jjh3+jjh4)/2+J+1 )%2?-1:1;
    }
  else
    {
      pos_ket = &CCD_position(k,l);
    }
  if (pos_bra->channel[index_bra] != -1&&pos_ket->channel[index_ket] != -1&&pos_bra->channel[index_bra]==pos_ket->channel[index_ket])
    return phase*factor*hhhh[pos_bra->channel[index_bra]].Mat(pos_bra->indice[index_bra],pos_ket->indice[index_ket]);
  else return 0;
}

bool CCD::Ladders()
{
  const int IterMax=300;
  vector<ccd_channel> CCD_T_old;
  int iter = 0;
  CCD_deltaE = 0.0;
  for(int i = 0; i < CCD_V_hhpp.size(); i++)
    {
      int twoj_plus_1=CCD_V_hhpp[i].J*2+1;
      CCD_deltaE +=twoj_plus_1*( CCD_V_hhpp[i].Mat*( CCD_T_hhpp[i].Mat.transpose() ) ).trace();
    }
  //  cout<<CCD_deltaE<<endl;
  double CCD_deltaE_old;
  do
    {
      iter++;
      CCD_deltaE_old = CCD_deltaE;
      CCD_T_old = CCD_T_hhpp;
      for(int i = 0; i < CCD_V_hhpp.size(); i++)
	{
	  CCD_T_hhpp[i].Mat = CCD_V_hhpp[i].Mat
	    + CCD_T_old[i].Mat * CCD_V_pppp[i].Mat
	    + CCD_V_hhhh[i].Mat * CCD_T_old[i].Mat;
	  CCD_T_hhpp[i].Mat = CCD_T_hhpp[i].Mat.cwiseProduct(CCD_e_hhpp[i].Mat);
	}
      double sum = 0;
      for(int i = 0; i < CCD_V_hhpp.size(); i++)
	{
	  int twoj_plus_1=CCD_V_hhpp[i].J*2+1;
	  sum += twoj_plus_1*( CCD_V_hhpp[i].Mat*( CCD_T_hhpp[i].Mat.transpose() ) ).trace();
	}
      CCD_deltaE = sum;
      //cout << iter << "\t" << sum << endl;
    }while(abs(CCD_deltaE_old - CCD_deltaE) > 1e-6 && iter<IterMax);
  if(iter>=IterMax) return false;
  return true;
}

bool CCD::Full()
{
  const int IterMax=300;
  vector<ccd_channel> CCD_T_old=CCD_T_hhpp;
  int iter = 0;
  CCD_deltaE = 0.0;
  for(int i = 0; i < CCD_V_hhpp.size(); i++)
    {
      int twoj_plus_1=CCD_V_hhpp[i].J*2+1;
      CCD_deltaE +=twoj_plus_1*( CCD_V_hhpp[i].Mat*( CCD_T_hhpp[i].Mat.transpose() ) ).trace();
    }
  //  cout<<CCD_deltaE<<endl;

  vector<ccd_channel> helpMatrix;

  vector<ccd_channel> CCD_intermediate_hhhh, CCD_intermediate_pppp;
  CCD_intermediate_pppp = CCD_V_pppp;
  CCD_intermediate_hhhh = CCD_V_hhhh;

  vector<ccd_channel> CCD_intermediate_phmhpm, CCD_intermediate_hpmhpm, CCD_T_phmhpm;
  CCD_intermediate_phmhpm = CCD_V_phmhpm;
  CCD_intermediate_hpmhpm = CCD_V_hpmhpm;
  CCD_T_phmhpm = CCD_V_phmhpm;

  double CCD_deltaE_old;
  double alpha=0.4;
  do
    {
      iter++;
      CCD_deltaE_old = CCD_deltaE;
      for(int i=0;i<CCD_T_hhpp.size();i++)
	CCD_T_old[i].Mat = alpha*CCD_T_hhpp[i].Mat+(1-alpha)*CCD_T_old[i].Mat;

      //pppp part
      //CCD_intermediate_pppp = CCD_V_pppp;
      
      //hhhh part
      helpMatrix=CCD_V_hhpp;
      for(int i = 0; i < CCD_V_hhhh.size(); i++)
	{
	  CCD_intermediate_hhhh[i].Mat = CCD_V_hhhh[i].Mat
	    + CCD_T_old[i].Mat * CCD_V_hhpp[i].Mat.transpose();
	}
      //TODO: this function is terribly long and hard to read, it can be improve by extracting some procedure to small specific functions.       
      //write T in form of phmhpm, 
      for (int channel = 0; channel < CCD_V_phmhpm.size(); channel++)
	{
	  int J=CCD_T_phmhpm[channel].J;
	  
	  for (int Ibra = 0; Ibra < CCD_V_phmhpm[channel].bra.size(); Ibra++)
	    for (int Iket = 0; Iket < CCD_V_phmhpm[channel].ket.size(); Iket++)
	      {
		CCD_T_phmhpm[channel].Mat(Ibra,Iket) = 0.0;
		TwoBodyStateType & State_bra = TwoBodyStates_phm[ CCD_V_phmhpm[channel].bra[Ibra] ];
		TwoBodyStateType & State_ket = TwoBodyStates_hpm[ CCD_V_phmhpm[channel].ket[Iket] ];
		int p1 = State_bra.a;
		int h1 = State_bra.b;
		int h2 = State_ket.a;
		int p2 = State_ket.b;
		int jjh1=HFOrbs[h1].jj;
		int jjp1=HFOrbs[p1].jj;
		int jjh2=HFOrbs[h2].jj;
		int jjp2=HFOrbs[p2].jj;
		int jmin_h=abs(jjh1-jjh2)/2;
		int jmax_h=(jjh1+jjh2)/2;
		int jmin_p=abs(jjp1-jjp2)/2;
		int jmax_p=(jjp1+jjp2)/2;
		int jmin=max(jmin_h,jmin_p);
		int jmax=min(jmax_h,jmax_p);
		int phase1=( (jjp2-jjh2)/2+J)%2?-1:1;
		for(int J_prime=jmin;J_prime<=jmax;J_prime++)
		  {
		    int phase2=( (jjh1+jjp2)/2+J+J_prime)%2?-1:1;
		    CCD_T_phmhpm[channel].Mat(Ibra,Iket)
		      += phase1*phase2*(2*J_prime+1)*gethhpp(h1,h2,p1,p2,J_prime,CCD_T_old)*gsl_sf_coupling_6j(jjp1,jjh1,J*2,jjh2,jjp2,2*J_prime);
		  }
	      }
	}
      
      for (int i = 0; i < CCD_V_phmhpm.size(); i++)
	{
	  CCD_intermediate_hpmhpm[i].Mat = CCD_V_hpmhpm[i].Mat + 0.5*CCD_V_phmhpm[i].Mat.transpose() * CCD_T_phmhpm[i].Mat;
	}
     
      helpMatrix = CCD_V_phmhpm;
      for (int i = 0; i < CCD_intermediate_hpmhpm.size(); i++)
	{
	  helpMatrix[i].Mat = CCD_T_phmhpm[i].Mat * CCD_intermediate_hpmhpm[i].Mat;
	}
      //calculations
      for (int i = 0; i < CCD_V_hhpp.size(); i++)
	{
	  CCD_T_hhpp[i].Mat = CCD_V_hhpp[i].Mat
	    + CCD_T_old[i].Mat * CCD_intermediate_pppp[i].Mat
	    + CCD_intermediate_hhhh[i].Mat * CCD_T_old[i].Mat;
	}
      //write back helpMatrix into T
      for (int channel = 0; channel < helpMatrix.size(); channel++)
	{
	  int J=helpMatrix[channel].J;
	  for (int Ibra = 0; Ibra < helpMatrix[channel].bra.size(); Ibra++)
	    for (int Iket = 0; Iket < helpMatrix[channel].ket.size(); Iket++)
	      {
		const TwoBodyStateType & State_bra = TwoBodyStates_phm[ helpMatrix[channel].bra[Ibra] ];
		const TwoBodyStateType & State_ket = TwoBodyStates_hpm[ helpMatrix[channel].ket[Iket] ];
		int p1 = State_bra.a;
		int h1 = State_bra.b;
		int h2 = State_ket.a;
		int p2 = State_ket.b;
		int jjh1=HFOrbs[h1].jj;
		int jjp1=HFOrbs[p1].jj;
		int jjh2=HFOrbs[h2].jj;
		int jjp2=HFOrbs[p2].jj;
		int jmin_h=abs(jjh1-jjh2)/2;
		int jmax_h=(jjh1+jjh2)/2;
		int jmin_p=abs(jjp1-jjp2)/2;
		int jmax_p=(jjp1+jjp2)/2;
		int jmin=max(jmin_h,jmin_p);
		int jmax=min(jmax_h,jmax_p);
		int phase1=( (jjp2-jjh2)/2+J)%2?-1:1;
		for(int J_prime=jmin;J_prime<=jmax;J_prime++)
		  {
		    int index_h=J_prime-jmin_h;
		    int index_p=J_prime-jmin_p;
		    double phase2=1.0;
		    if(p1==p2 && (J_prime%2) ) continue;
		    if(h1==h2 && (J_prime%2) ) continue;
		    position * pos_pp;
		    position * pos_hh;
		    if(p1>p2)
		      {
			pos_pp = & CCD_position(p2,p1);
			phase2*=( (jjp1+jjp2)/2 + J_prime + 1)%2?-1:1;
		      }
		    else
		      {
			pos_pp=&CCD_position(p1,p2);
		      }
		    if(h1>h2)
		      {
			pos_hh = &CCD_position(h2,h1);
			phase2*=( (jjh1+jjh2)/2 + J_prime + 1)%2?-1:1;
		      }
		    else
		      {
			pos_hh=&CCD_position(h1,h2);
		      }
		    double factor=1.0;
		    //factor for P(ab),P(ij), if(p1==p2||h1==h2)
		    if(p1==p2) factor*=2;
		    if(h1==h2) factor*=2;
		    //factor for hhpp
		    if(p1==p2) factor*=sqrt(0.5);
		    if(h1==h2) factor*=sqrt(0.5);
		    phase2*=( (jjh1+jjp2)/2+J+J_prime)%2?-1:1;
		    if ( (pos_pp->channel[index_p] == pos_hh->channel[index_h])&&(pos_pp->channel[index_p] != -1) )
		      CCD_T_hhpp[pos_pp->channel[index_p]].Mat(pos_hh->indice[index_h],pos_pp->indice[index_p])
			+= factor*phase1*phase2*(2*J+1)*helpMatrix[channel].Mat(Ibra,Iket)*gsl_sf_coupling_6j(jjp2,jjh2,J*2,jjh1,jjp1,2*J_prime);
		  }
	      }
	}
      // //one body nonlinear contri. particle part
      helpMatrix=CCD_V_pppp;
      for(int i = 0; i < CCD_V_pppp.size(); i++)
      	{
      	  helpMatrix[i].Mat = CCD_T_old[i].Mat.transpose() * CCD_V_hhpp[i].Mat;
      	}
      for(int channel=0;channel<CCD_V_hhpp.size();channel++)
        {
          int Jhhpp=CCD_V_hhpp[channel].J;
          for(int Ibra=0;Ibra<CCD_V_hhpp[channel].bra.size();Ibra++)
            for(int Iket=0;Iket<CCD_V_hhpp[channel].ket.size();Iket++)
      	      {
      		const TwoBodyStateType & State_bra = TwoBodyStates_hh[ CCD_V_hhpp[channel].bra[Ibra] ];
      		const TwoBodyStateType & State_ket = TwoBodyStates_pp[ CCD_V_hhpp[channel].ket[Iket] ];
      		int i=State_bra.a;
      		int j=State_bra.b;
      		int a=State_ket.a;
      		int b=State_ket.b;
      		double factor=1.;
      		if(a==b) factor*=sqrt(0.5);
      		if(i==j) factor*=sqrt(0.5);
      		int jjb=HFOrbs[b].jj;
      		int jja=HFOrbs[a].jj;
      		for (int c = FermiSurface+1; c < totalOrbitals; c++)
      		  {
      		    int jjc=HFOrbs[c].jj;
      		    if(jjc!=jjb) continue;
      		    double temp=0;
      		    for (int d = FermiSurface+1; d < totalOrbitals; d++)
      		      {
      			int jjd=HFOrbs[d].jj;
      			int jmin=abs(jjb-jjd)/2;
      			int jmax=(jjb+jjd)/2;
      			for(int J=jmin;J<=jmax;J++)
      			  {
      			    temp +=
      			      -(2*J+1.)/(jjb+1) * getpppp(b,d,c,d,J,helpMatrix);
      			  }
      		      }
      		    CCD_T_hhpp[channel].Mat(Ibra,Iket) +=factor*gethhpp(i,j,a,c,Jhhpp,CCD_T_old)*temp;
      		  }
		//exchage of a,b
      		int phase=( (jja+jjb)/2 + Jhhpp +1)%2?-1:1;
      		for (int c = FermiSurface+1; c < totalOrbitals; c++)
      		  {
      		    int jjc=HFOrbs[c].jj;
      		    if(jjc!=jja) continue;
      		    double temp=0;
      		    for (int d = FermiSurface+1; d < totalOrbitals; d++)
      		      {
      			int jjd=HFOrbs[d].jj;
      			int jmin=abs(jja-jjd)/2;
      			int jmax=(jja+jjd)/2;
      			for(int J=jmin;J<=jmax;J++)
      			  {
      			    temp +=
      			      -(2*J+1.)/(jja+1) * getpppp(a,d,c,d,J,helpMatrix);
      			  }
      		      }
      		    CCD_T_hhpp[channel].Mat(Ibra,Iket) +=factor*gethhpp(i,j,b,c,Jhhpp,CCD_T_old)*temp*phase;
      		  }
      	      }
        }
      // //one body nonlinear contri. hole part
      helpMatrix = CCD_V_hhhh;
      for(int i = 0; i < CCD_V_hhpp.size(); i++)
	{
	  helpMatrix[i].Mat = CCD_T_old[i].Mat * CCD_V_hhpp[i].Mat.transpose();
	}
      for(int channel=0;channel<CCD_V_hhpp.size();channel++)
        {
          int Jhhpp=CCD_V_hhpp[channel].J;
          for(int Ibra=0;Ibra<CCD_V_hhpp[channel].bra.size();Ibra++)
            for(int Iket=0;Iket<CCD_V_hhpp[channel].ket.size();Iket++)
      	      {
      		const TwoBodyStateType & State_bra = TwoBodyStates_hh[ CCD_V_hhpp[channel].bra[Ibra] ];
      		const TwoBodyStateType & State_ket = TwoBodyStates_pp[ CCD_V_hhpp[channel].ket[Iket] ];
      		int i=State_bra.a;
      		int j=State_bra.b;
      		int a=State_ket.a;
      		int b=State_ket.b;
      		double factor=1.;
      		if(a==b) factor*=sqrt(0.5);
      		if(i==j) factor*=sqrt(0.5);
      		int jji=HFOrbs[i].jj;
      		int jjj=HFOrbs[j].jj;
      		for (int k = 0; k <= FermiSurface; k++)
      		  {
      		    int jjk=HFOrbs[k].jj;
      		    if(jjk!=jjj) continue;
      		    double temp=0;
      		    for (int l = 0; l<=FermiSurface; l++)
      		      {
      			int jjl=HFOrbs[l].jj;
      			int jmin=abs(jjj-jjl)/2;
      			int jmax=(jjj+jjl)/2;
      			for(int J=jmin;J<=jmax;J++)
      			  {
      			    temp +=
      			      (2*J+1.)/(jjj+1) * gethhhh(k,l,j,l,J,helpMatrix);
      			  }
      		      }
      		    CCD_T_hhpp[channel].Mat(Ibra,Iket) -=factor*gethhpp(i,k,a,b,Jhhpp,CCD_T_old)*temp;
      		  }
		//exchange of i,j
      		int phase=( (jji+jjj)/2 + Jhhpp +1)%2?-1:1;
      		for (int k = 0; k <= FermiSurface; k++)
      		  {
      		    int jjk=HFOrbs[k].jj;
      		    if(jjk!=jji) continue;
      		    double temp=0;
		    for (int l = 0; l<=FermiSurface; l++)
      		      {
      			int jjl=HFOrbs[l].jj;
      			int jmin=abs(jji-jjl)/2;
      			int jmax=(jji+jjl)/2;
      			for(int J=jmin;J<=jmax;J++)
      			  {
      			    temp +=
      			      (2*J+1.)/(jji+1) * getpppp(k,l,i,l,J,helpMatrix);
      			  }
      		      }
      		    CCD_T_hhpp[channel].Mat(Ibra,Iket) -=factor*gethhpp(j,k,a,b,Jhhpp,CCD_T_old)*temp*phase;
      		  }
      	      }
        }

      for (int i = 0; i < CCD_V_hhpp.size(); i++)
	{
	  for (int Ibra = 0; Ibra < CCD_T_old[i].bra.size(); Ibra++)
	    for (int Iket = 0; Iket < CCD_T_old[i].ket.size(); Iket++)
	      CCD_T_hhpp[i].Mat(Ibra,Iket) /=
		+ HFOrbs[TwoBodyStates_hh[ CCD_T_old[i].bra[Ibra] ].a].e
		+ HFOrbs[TwoBodyStates_hh[ CCD_T_old[i].bra[Ibra] ].b].e
		- HFOrbs[TwoBodyStates_pp[ CCD_T_old[i].ket[Iket] ].a].e
		- HFOrbs[TwoBodyStates_pp[ CCD_T_old[i].ket[Iket] ].b].e;
	}
      double sum = 0;
      for(int i = 0; i < CCD_V_hhpp.size(); i++)
	{
	  int twoj_plus_1=CCD_V_hhpp[i].J*2+1;
	  sum += (CCD_V_hhpp[i].Mat*(CCD_T_hhpp[i].Mat.transpose())).trace() * twoj_plus_1;
	}
      CCD_deltaE = sum;
      //      cout << iter << "\t"  << sum << endl;
    }while (abs(CCD_deltaE_old - CCD_deltaE) > 1e-6 && iter<IterMax);
  if(iter>=IterMax) return false;
  return true;
}
double CCD::TwoOrder_2p2h()
{
  double dE=0;
  for(int i = 0; i < CCD_V_hhpp.size(); i++)
    {
      int twoj_plus_1=CCD_V_hhpp[i].J*2+1;
      dE += (CCD_e_hhpp[i].Mat.cwiseProduct( CCD_V_hhpp[i].Mat ) * (CCD_V_hhpp[i].Mat.transpose()) ).trace() * twoj_plus_1;
    }
  return dE;
}
